#include <opencv2/opencv.hpp>
#include <iostream>
#include <chrono>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <stdio.h>
#include <memory>
#include <sys/time.h>
#include <atomic>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>

#include <image_transport/image_transport.h>

#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "kbt_cam/rknnPool.hpp"
#include "kbt_cam/osiris.hpp"
#include "kbt_cam/RtspCapture.hpp"

#include <swarm_msgs/BoundingBoxes.h>
#include <swarm_msgs/BoundingBox.h>

#include <unordered_map>
#include <vector>
#include <sensor_msgs/NavSatFix.h>

const int MAX_CACHE_SIZE = 20;  // 可根据实际内存容量调整

std::mutex cache_mutex; // 如果多线程使用，需加锁保护
std::unordered_map <uint64_t, std::vector<InferenceResult>> frame_cache;

int8_t vechile_state = 0;
std::atomic<int> cam_state{0};;
double vechile_yaw = 0.0;
double gun_yaw = 0.0;

std::atomic<bool> running(true);
std::atomic<bool> running_far(true);
std::atomic<bool> running_close(true);
std::atomic<bool> publish_running{true};

std::mutex frame_mtx;
cv::Mat shared_frame;
cv::Mat rtsp_frame;

std::mutex frame_far_mtx;
cv::Mat shared_frame_far;

std::atomic <uint64_t> latestFrameId = {0};     // 每帧递增
std::atomic <uint64_t> lastPushedFrameId = {0}; // 上一次压入的帧ID

std::atomic <uint64_t> latestFrameId_far = {0};     // 每帧递增
std::atomic <uint64_t> lastPushedFrameId_far = {0}; // 上一次压入的帧ID


std::deque <std::pair<int, cv::Mat>> original_frame_cache;
std::mutex original_frame_mtx;

// 全局或局部定义，保存预设的 6 个窗口区域
std::vector <cv::Rect> roi_windows = {
        cv::Rect(0, 0, 740, 640),     // 窗口1
        cv::Rect(590, 0, 740, 640),   // 窗口2
        cv::Rect(1180, 0, 740, 640),   // 窗口3
        cv::Rect(0, 440, 740, 640),   // 窗口4
        cv::Rect(590, 440, 740, 640), // 窗口5
        cv::Rect(1180, 440, 740, 640)  // 窗口6
};


// std::chrono::high_resolution_clock::time_point start;

void vehiclestateCallback(const sensor_msgs::NavSatFix &msg) {
    double uav_dis = -1.0;
    vechile_yaw = msg.position_covariance[2];
    gun_yaw = msg.position_covariance[3];
    vechile_state = msg.position_covariance[4];
    uav_dis = msg.position_covariance[5];

    // uav_dis = 150;
    if (uav_dis > 100.0){
        cam_state = 2;
        running_far = true;
        running_close = false;
    } 
    // else if (uav_dis > 50.0){
    //     cam_state = 1;
    //     running_far = true;
    //     running_close = true;
    // }
        
    else{
        running_far = false;
        running_close = true;
        cam_state = 0;
    }
        

    // cam_state = 2;
    // std::cout<<"--->camstate: "<<cam_state<<"running_far"<<running_far<<std::endl;
}

// 设置线程绑定到特定核心
void bind_to_core(int core_id) {
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(core_id, &cpuset);
    pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);
}

// 采集线程函数
void capture_thread(cv::VideoCapture &cap) {
    bind_to_core(7);  // 采集线程绑定到 CPU7
    cv::Mat frame;
    // std::cout << "running"<<running << std::endl;
    while(true){
        if (!cap.read(frame)) {
            std::cerr << "Failed to read near frame!" << std::endl;
            continue;
        } else if (running_close && cam_state != 2) {
            std::lock_guard <std::mutex> lock(frame_mtx);
            shared_frame = frame.clone(); // 图像数据复制
            latestFrameId++;
        
        }
    }  
}

void capture_far_thread(cv::VideoCapture &cap_far) {
    bind_to_core(6);  // 采集线程绑定到 CPU6
    cv::Mat frame_far;
    // std::cout << "running_far"<<running_far << std::endl;
    while(true){
        // std::cout << "star far" << std::endl;
        if (!cap_far.read(frame_far)) {
            std::cout << "Failed to read far frame!" << std::endl;
            continue;
        } else if (running_far && cam_state != 0) {
            // std::cout << "frame far" << std::endl;
            std::lock_guard <std::mutex> lock(frame_far_mtx);
            shared_frame_far = frame_far.clone(); // 图像数据复制
            latestFrameId_far++;
        }
    }
}

template<typename ModelType>
void publish_thread_func(
        rknnPool<ModelType, WindowInput, InferenceResult> &camPool,
        ros::Publisher &result_pub,
        std::atomic<bool> &publish_running, 
        image_transport::Publisher &image_pub
) {
    // cv::namedWindow("DetectedFrame", cv::WINDOW_AUTOSIZE);
    int frames = 0;

    struct timeval time;
    gettimeofday(&time, nullptr);
    auto startTime = time.tv_sec * 1000 + time.tv_usec / 1000;
    auto beforeTime = startTime;

    auto before_nearTime = startTime;

    const std::array<std::string, 6> kWinNames = {
            "Result1", "Result2", "Result3",
            "Result4", "Result5", "Result6"
    };

    // for (const auto& n : kWinNames) {
    //     cv::namedWindow(n, cv::WINDOW_NORMAL);
    // }
    // cv::namedWindow(n, cv::WINDOW_NORMAL);
    while (publish_running.load()) {
        InferenceResult result;
        if (camPool.get(result) == 0) {
            // std_msgs::Header header;
            // header.stamp = ros::Time::now();
            // sensor_msgs::ImagePtr msg = cv_bridge::CvImage(header, "bgr8", result).toImageMsg();
            // result_pub.publish(msg);
            std::lock_guard <std::mutex> lock(cache_mutex);

            // std::cout << result.frame_id << std::endl;
            // 放入缓存
            auto &vec = frame_cache[result.frame_id];
            vec.push_back(result);

            // 如果已满6张图
            if (vec.size() == 6) {
                uint64_t Current_frame_id = 0;
                Current_frame_id = result.frame_id;
                auto current_nearTime = time.tv_sec * 1000 + time.tv_usec / 1000;
                if (Current_frame_id > 100000 && current_nearTime - before_nearTime < 500)
                    break;

                // 🔍 查找所有图中置信度最大的目标
                const InferenceResult *best_result = nullptr;
                double best_prob = -1.0, max_dis = -1.0, now_dis = -1.0, dis_x = -1.0, dis_y = -1.0;

                swarm_msgs::BoundingBox bb;

                for (const auto &res: vec) {
                    if (res.has_target) {
                        const cv::Rect &roi_now = roi_windows[res.window_id - 1];
                        dis_x = (res.x1 + res.x2) / 2 + roi_now.x - 960;
                        dis_y = (res.y1 + res.y2) / 2 + roi_now.y - 540;
                        now_dis = sqrt(dis_x * dis_x + dis_y * dis_y);
                        if (now_dis > max_dis) {
                            max_dis = now_dis;
                            best_result = &res;
                        }
                        // if (res.prop > best_prob) {
                        //     best_prob = res.prop;
                        //     best_result = &res;
                        // }
                    }
//                    else
//                        Current_frame_id = res.frame_id;
                }

                if (best_result) {
                    int window_id = best_result->window_id; // ∈ [1, 6]
                    int offset_x = 0, offset_y = 0;
//                    Current_frame_id = best_result->frame_id;

                    // 安全取偏移量
                    if (window_id >= 1 && window_id <= 6) {
                        const cv::Rect &roi = roi_windows[window_id - 1];
                        offset_x = roi.x;
                        offset_y = roi.y;
                    }
                    // 发布该帧的最大框
                    swarm_msgs::BoundingBoxes msg;
                    msg.header.stamp = ros::Time::now();
                    msg.header.frame_id = "camera";


                    bb.probability = best_result->prop;
                    bb.xmin = best_result->x1 + offset_x;
                    bb.ymin = best_result->y1 + offset_y;
                    bb.xmax = best_result->x2 + offset_x;
                    bb.ymax = best_result->y2 + offset_y;
                    // bb.id   = best_box.id;
                    // bb.class_name = best_box.class_name;
                    // bb.point = best_box.point;

                    msg.bounding_boxes.push_back(bb);
                    result_pub.publish(msg);
                }

                {
                    // printf("123\n");
                    std::lock_guard <std::mutex> lock(original_frame_mtx);
                    for (auto it = original_frame_cache.rbegin(); it != original_frame_cache.rend(); ++it) {
                        if (it->first == Current_frame_id) {
                            cv::Mat raw_img = it->second.clone();

                            if (best_result) {
                                // 画矩形框
                                cv::rectangle(raw_img,
                                              cv::Point(bb.xmin, bb.ymin),
                                              cv::Point(bb.xmax, bb.ymax),
                                              cv::Scalar(0, 255, 0), 2); // 绿色框，线宽 2
                                int cx = (bb.xmin + bb.xmax) / 2;
                                int cy = (bb.ymin + bb.ymax) / 2;
                                cv::line(raw_img, cv::Point(cx, cy), cv::Point(raw_img.cols / 2, raw_img.rows / 2),
                                         cv::Scalar(0, 0, 255), 1); // 水平红线
                                putText(raw_img, "uav", cv::Point(bb.xmin, bb.ymin - 20), cv::FONT_HERSHEY_SIMPLEX, 1,
                                        cv::Scalar(255, 0, 0), 2);
                                if (vechile_state == 1)
                                    putText(raw_img, "track", cv::Point(raw_img.cols - 350, raw_img.rows - 100),
                                            cv::FONT_HERSHEY_SIMPLEX, 3, cv::Scalar(255, 0, 0), 3);
                                else if (vechile_state == 2)
                                    putText(raw_img, "attack", cv::Point(raw_img.cols - 350, raw_img.rows - 100),
                                            cv::FONT_HERSHEY_SIMPLEX, 3, cv::Scalar(255, 0, 0), 3);
                                
                            }

                            // 计算目标中心
                            putText(raw_img, "VY:"+std::to_string(vechile_yaw*180/3.14159265), cv::Point(raw_img.cols - 400, raw_img.rows - 400),
                                            cv::FONT_HERSHEY_SIMPLEX, 3, cv::Scalar(255, 0, 0), 3);
                            putText(raw_img, "GY:"+std::to_string(gun_yaw*180/3.14159265), cv::Point(raw_img.cols - 400, raw_img.rows - 300),
                                            cv::FONT_HERSHEY_SIMPLEX, 3, cv::Scalar(255, 0, 0), 3);

                            // 画十字（中心到四边）
                            cv::line(raw_img, cv::Point(raw_img.cols / 2, 0),
                                     cv::Point(raw_img.cols / 2, raw_img.rows - 1), cv::Scalar(0, 0, 255), 1); // 垂直红线
                            cv::line(raw_img, cv::Point(0, raw_img.rows / 2),
                                     cv::Point(raw_img.cols - 1, raw_img.rows / 2), cv::Scalar(0, 0, 255), 1); // 水平红线

                            // // 可选：画中心点
                            // cv::circle(raw_img, cv::Point(cx, cy), 4, cv::Scalar(255, 0, 0), -1); // 蓝色中心点



                            // cv::resize(raw_img, Detected_Frame, cv::Size(), 0.5, 0.4, cv::INTER_LINEAR);

                            // cv::Rect roi_dis(960,900,1920,1080);
                            // cv::Mat Detected_Frame = raw_img(roi_dis);
                            cv::Mat resized_image;
                            cv::Size dsize(1920, 1150);
                            cv::resize(raw_img, resized_image, dsize, 0, 0, cv::INTER_AREA);
                            // 显示图像
                            cv::Mat display = resized_image.clone();
                            bool have_rtsp = !rtsp_frame.empty();

                            if (have_rtsp)
                            {
                                // ====== 1. 缩小 RTSP 图像，作为右上角小画面 ======
                                int H = display.rows;
                                int W = display.cols;

                                // 小画面高度 = 主画面高度的 1/4
                                int pip_h = H / 4;
                                int rtsp_h = rtsp_frame.rows;
                                int rtsp_w = rtsp_frame.cols;

                                if (rtsp_h > 0 && rtsp_w > 0)
                                {
                                    int pip_w = static_cast<int>( (double)rtsp_w / rtsp_h * pip_h );

                                    cv::Mat pip;
                                    cv::resize(rtsp_frame, pip, cv::Size(pip_w, pip_h), 0, 0, cv::INTER_AREA);

                                    int margin = 10;
                                    int x1 = W - pip_w - margin;  // 右上角 x
                                    int y1 = margin;              // 右上角 y

                                    // 边界防护（一般不太会超，但以防万一）
                                    if (x1 < 0) x1 = 0;
                                    if (y1 < 0) y1 = 0;
                                    if (x1 + pip_w > W) pip_w = W - x1;
                                    if (y1 + pip_h > H) pip_h = H - y1;

                                    cv::Rect roi(x1, y1, pip_w, pip_h);
                                    cv::Mat dst_roi = display(roi);

                                    // 如果上面因为边界截断了，就同步裁剪 pip
                                    cv::Mat pip_crop = pip(cv::Rect(0, 0, pip_w, pip_h));

                                    // 直接拷贝小画面到主图右上角
                                    pip_crop.copyTo(dst_roi);
                                }
                            }

                            // ====== 2. 显示最终画面（有 RTSP 就叠加，没有就只有主图） ======
                            cv::imshow("DetectedFrame", display);
                            cv::moveWindow("DetectedFrame", 0, 0);

                            // sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", resized_image).toImageMsg();
                            
                            // if (!rtsp.getFrame(rtsp_frame))
                            // image_pub.publish(msg);
                            
                            // cv::imshow("DetectedFrame", resized_image);
                            // cv::moveWindow("DetectedFrame", 0, 0);
                            // cv::waitKey(1);

                            break; // 找到了就退出
                        }
                    }
                }


                // 🔁 清理该 frame_id
                frame_cache.erase(Current_frame_id);
            }
            frames++;
            if (frames % 60 == 0) {
                gettimeofday(&time, nullptr);
                auto currentTime = time.tv_sec * 1000 + time.tv_usec / 1000;
                printf("60帧内平均帧率:\t %f fps/s\n", 60.0 / float(currentTime - beforeTime) * 1000.0);
                beforeTime = currentTime;
            }


            // std::cout << result.has_target <<std::endl;
            // if (result.window_id >= 1 && result.window_id <= 6) {
            //     int idx = result.window_id - 1;
            //     const auto& rect = roi_windows[idx];          // 读取 ROI 位置
            //     const auto& win  = kWinNames[idx];            // 对应窗口名

            //     cv::imshow(win, result.result);               // 显示图像
            //     cv::moveWindow(win, rect.x, rect.y);          // 放到桌面指定坐标
            //     cv::resizeWindow(win, rect.width, rect.height); // 可选：确保大小一致
            // } else {
            //     // 异常 ID，退而求其次
            //     cv::imshow("Result", result.result);
            // }
            // cv::imshow("Result", result.result);
            // if (result.has_target) {
            //     // 发布或显示目标
            //     // cv::imshow("Result", result.result);
            //     // continue;
            // } 
            // else {
            //     // 可以选择跳过发布或跳过显示

            // }
            // cv::imshow("Result", result.result);
            if (cv::waitKey(1) == 'q') // 延时1毫秒,按q键退出/Press q to exit
            {
                running = false;
                break;
            }
        } else {
            // std::cout << "123\n"<<std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }

    gettimeofday(&time, nullptr);
    auto endTime = time.tv_sec * 1000 + time.tv_usec / 1000;

    printf("Average:\t %f fps/s\n", float(frames) / float(endTime - startTime) * 1000.0);

}

// 显式实例化模板（避免链接错误）
template void publish_thread_func<osiris>(
        rknnPool<osiris, WindowInput, InferenceResult> &,
        ros::Publisher &,
        std::atomic<bool> &,
        image_transport::Publisher &
);

void image_push_thread_func(rknnPool<osiris, WindowInput, InferenceResult> &camPool) {
    while (true) {
        uint64_t currentFrameId;
        uint64_t currentFrameId_far;
        int window_id = 0;
        if (running_close){
            // std::cout << "running" <<std::endl;
            {
                std::lock_guard <std::mutex> lock(frame_mtx);
                if (shared_frame.empty()) continue;

                currentFrameId = latestFrameId.load();

                if (currentFrameId == lastPushedFrameId.load()) {
                    continue;
                }

                // 存储原始图像
                {
                    std::lock_guard <std::mutex> orig_lock(original_frame_mtx);
                    original_frame_cache.emplace_back(currentFrameId, shared_frame.clone());

                    while (original_frame_cache.size() > MAX_CACHE_SIZE) {
                        original_frame_cache.pop_front();
                    }
                }


                for (const auto &roi: roi_windows) {
                    if ((roi.x + roi.width <= shared_frame.cols) &&
                        (roi.y + roi.height <= shared_frame.rows)) {
                        WindowInput win_input;
                        window_id += 1;
                        win_input.window_id = window_id;
                        win_input.frame_id = currentFrameId;
                        win_input.window_img = shared_frame(roi).clone();
                        if (camPool.put(win_input) != 0) {
                            std::cerr << "camPool.put failed for roi: " << roi << "\n";
                        }
                    } else {
                        std::cerr << "Invalid ROI: " << roi << " exceeds image size\n";
                    }
                }

                lastPushedFrameId.store(currentFrameId);
            }
            }
        if (running_far){
            {
                // std::cout << "far_running" <<std::endl;
                std::lock_guard <std::mutex> lock(frame_far_mtx);
                if (shared_frame_far.empty()) 
                // {
                    // std::cout << "far - empty" <<std::endl;
                    continue;
                // }else{
                //     // std::cout << "far - get" <<std::endl;
                // }
                // cv::imshow("img", shared_frame_far);
                // cv::waitKey(1);
                currentFrameId_far = latestFrameId_far.load();


                if (currentFrameId_far == lastPushedFrameId_far.load()) {
                    continue;
                }

                // 存储原始图像
                {
                    std::lock_guard <std::mutex> orig_lock(original_frame_mtx);
                    original_frame_cache.emplace_back(currentFrameId_far + 50000, shared_frame_far.clone());

                    while (original_frame_cache.size() > MAX_CACHE_SIZE) {
                        original_frame_cache.pop_front();
                    }
                }

                window_id = 0;
                // cv::imshow("img", shared_frame_far.clone());
                for (const auto &roi: roi_windows) {
                    if ((roi.x + roi.width <= shared_frame_far.cols) &&
                        (roi.y + roi.height <= shared_frame_far.rows)) {
                        WindowInput win_input;
                        window_id += 1;
                        win_input.window_id = window_id;
                        win_input.frame_id = currentFrameId_far + 50000;
                        win_input.window_img = shared_frame_far(roi).clone();
                        if (camPool.put(win_input) != 0) {
                            std::cout << "camPool.put failed for roi: " << roi <<std::endl;;
                        }
                        else
                        {
                            std::cout<<"campool_true"<<std::endl;
                        }
                    } else {
                        std::cout << "Invalid ROI: " << roi << " exceeds image size\n";
                    }
                }

                lastPushedFrameId_far.store(currentFrameId_far);
            }
        }
    }
}


int main(int argc, char **argv) {
    ros::init(argc, argv, "kbt_cam_node");
    ros::NodeHandle nh;
    ros::Publisher result_pub = nh.advertise<swarm_msgs::BoundingBoxes>("/yindao/capture_drone", 10);
    image_transport::ImageTransport it(nh);
    image_transport::Publisher image_pub = it.advertise("/shouchi/image", 10);


    ros::Subscriber vechile_state_sub = nh.subscribe("/vehicle/position/global", 10, vehiclestateCallback);

    ROS_INFO("kbt_cam_node started.");

    /***************************************初始化摄像头开启采集线程***********************************************/
    cv::VideoCapture capture("/dev/video0", cv::CAP_V4L2);
    cv::VideoCapture capture_far("/dev/video2", cv::CAP_V4L2);

    if (!capture.isOpened()) {
        std::cout << "❌ 无法打开摄像头" << std::endl;
        return -1;
    }
    capture.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    capture.set(cv::CAP_PROP_FRAME_WIDTH, 1920);
    capture.set(cv::CAP_PROP_FRAME_HEIGHT, 1080);
    capture.set(cv::CAP_PROP_FPS, 10);
    std::cout << "分辨率: " << capture.get(cv::CAP_PROP_FRAME_WIDTH) << "x"
              << capture.get(cv::CAP_PROP_FRAME_HEIGHT) << "\n";

    if (!capture_far.isOpened()) {
        std::cout << "❌ 无法打开摄像头2" << std::endl;
        return -1;
    }
    capture_far.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    capture_far.set(cv::CAP_PROP_FRAME_WIDTH, 1920);
    capture_far.set(cv::CAP_PROP_FRAME_HEIGHT, 1080);
    capture_far.set(cv::CAP_PROP_FPS, 10);
    std::cout << "capture_far分辨率: " << capture_far.get(cv::CAP_PROP_FRAME_WIDTH) << "x"
              << capture_far.get(cv::CAP_PROP_FRAME_HEIGHT) << "\n";

    std::thread cap_thread(capture_thread, std::ref(capture));

    std::thread cap_far_thread(capture_far_thread, std::ref(capture_far));

    const std::string RTSP_URL = "rtsp://192.168.1.50:8554/air";

    const std::string pipeline =
        "rtspsrc location=" + RTSP_URL + " protocols=tcp latency=0 ! "
        "rtph264depay ! h264parse ! avdec_h264 ! "
        "videoconvert ! appsink drop=true max-buffers=1 sync=false";

    /***************************************参数初始化***********************************************/
    char *model_name = (char *) "/home/orangepi/kbt_ws/src/kbtdj_ground/kbt_cam/model/1119.rknn";

    int threadNum = 12;
    rknnPool<osiris, WindowInput, InferenceResult> camPool(model_name, threadNum);
    if (camPool.init() != 0) {
        printf("rknnPool init fail!\n");
        running = false;
        running_far = false;
        // cap_thread.join();
        cap_far_thread.join();
        return -1;
    }else
        running =true;

    std::thread publish_thread([&]() {
        publish_thread_func(camPool, result_pub, publish_running, image_pub);
    });

    std::thread image_push_thread([&]() {
        image_push_thread_func(camPool);
    });

            
    RtspCapture rtsp(pipeline);
    rtsp.start();  // 后台线程开始抓图

    // while (running) {
    //     cv::Mat input_img;
    //     uint64_t currentFrameId;

    //     {
    //         std::lock_guard<std::mutex> lock(frame_mtx);
    //         if (shared_frame.empty()) continue;

    //         currentFrameId = latestFrameId.load();

    //         // 如果当前帧已经被处理过，就跳过
    //         if (currentFrameId == lastPushedFrameId.load()) {
    //             continue;
    //         }

    //         // 存储原始图像
    //         {
    //             std::lock_guard<std::mutex> orig_lock(original_frame_mtx);
    //             original_frame_cache.emplace_back(currentFrameId, shared_frame.clone());

    //             // 控制缓存大小，避免内存膨胀
    //             while (original_frame_cache.size() > MAX_CACHE_SIZE) {
    //                 original_frame_cache.pop_front();
    //             }
    //         }

    //         // cv::imshow("img", shared_frame.clone());
    //         int window_id = 0;
    //         for (const auto& roi : roi_windows) {
    //             // 检查 ROI 是否在图像内
    //             if ((roi.x + roi.width <= shared_frame.cols) &&
    //                 (roi.y + roi.height <= shared_frame.rows)) {
    //                 WindowInput win_input;
    //                 window_id += 1;
    //                 win_input.window_id = window_id;
    //                 win_input.frame_id = currentFrameId;
    //                 win_input.window_img = shared_frame(roi).clone();
    //                 if (camPool.put(win_input) != 0) {
    //                     std::cerr << "camPool.put failed for roi: " << roi << "\n";
    //                 }
    //             } 
    //             else {
    //                 std::cerr << "Invalid ROI: " << roi << " exceeds image size\n";
    //             }
    //         }
    //         // WindowInput win_input;
    //         // win_input.window_id = window_id;
    //         // win_input.frame_id = currentFrameId;
    //         // win_input.window_img = shared_frame.clone();

    //         // if (camPool.put(win_input) != 0)
    //         //     break;


    //         lastPushedFrameId.store(currentFrameId);
    //         // // auto read = std::chrono::high_resolution_clock::now();
    //         // // auto diff = std::chrono::duration_cast<std::chrono::milliseconds>(read - start);
    //         // std::cout << "currentFrameId: " << currentFrameId << std::endl;
    //     }

    //     ros::spin();


    //     // frames++;

    //     // if (frames % 120 == 0)
    //     // {
    //     //     gettimeofday(&time, nullptr);
    //     //     auto currentTime = time.tv_sec * 1000 + time.tv_usec / 1000;
    //     //     printf("120帧内平均帧率:\t %f fps/s\n", 120.0 / float(currentTime - beforeTime) * 1000.0);
    //     //     beforeTime = currentTime;
    //     // }
    // }

    // while (true)
    // {
    //     cv::Mat img;
    //     if (camPool.get(img) != 0)
    //         break;
    //     cv::imshow("Result", img);
    //     if (cv::waitKey(1) == 'q') // 延时1毫秒,按q键退出/Press q to exit
    //         break;
    //     frames++;
    // }

    // gettimeofday(&time, nullptr);
    // auto endTime = time.tv_sec * 1000 + time.tv_usec / 1000;

    // printf("Average:\t %f fps/s\n", float(frames) / float(endTime - startTime) * 1000.0);

    /***** ROS 主线程用于处理回调 *****/
    // ros::spin();  // ✅ 会持续运行并响应 vehiclestateCallback 等回调
    // const int SCREEN_W = 1920;
    // const int SCREEN_H = 1200;
    // cv::Mat display;
    while(ros::ok()){

        ros::spinOnce();
        // rate.sleep();
        if (!rtsp.getFrame(rtsp_frame)){
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            continue;
        }
        // rtsp.getFrame(rtsp_frame);
        // std::this_thread::sleep_for(std::chrono::milliseconds(1));

        // int h = frame.rows;
        // int w = frame.cols;
        // if (h <= 0 || w <= 0)
        //     continue;

        // // ---- 计算缩放后的图像大小（只缩放图像，不动窗口） ----
        // double scale_w = static_cast<double>(SCREEN_W) / w;
        // double scale_h = static_cast<double>(SCREEN_H) / h;
        // double scale   = std::min(1.0, std::min(scale_w, scale_h));  // 不放大，只缩小

        // int dst_w = static_cast<int>(w * scale);
        // int dst_h = static_cast<int>(h * scale);

        
        // if (scale != 1.0)
        // {
        //     cv::resize(display, rtsp_frame, cv::Size(dst_w, dst_h), 0, 0, cv::INTER_AREA);
        // }
        // else
        // {
        //     rtsp_frame = display;  // 不需要缩放，直接用原图
        // }
        // int h = frame.rows;
        // int w = frame.cols;
        // if (!moved && h > 0 && w > 0)
        // {
        //     double scale_w = static_cast<double>(SCREEN_W) / w;
        //     double scale_h = static_cast<double>(SCREEN_H) / h;
        //     double scale   = std::min(1.0, std::min(scale_w, scale_h));

        //     int win_w = static_cast<int>(w * scale);
        //     int win_h = static_cast<int>(h * scale);

        //     cv::resizeWindow("RTSP Viewer", win_w, win_h);
        //     // cv::moveWindow("RTSP Viewer", SCREEN_W - win_w, 0);

        //     // moved = true;
        // }

        // cv::imshow("RTSP Viewer", frame);
        // if (cv::waitKey(1) == 'q')
        //     break;

    }

    // 退出时 join 所有线程
    running = false;
    running_far = false;
    publish_running = false;

    // cap_thread.join();
    cap_far_thread.join();
    publish_thread.join();

    rtsp.stop();

    capture.release();
    capture_far.release();
    cv::destroyAllWindows();

    return 0;
}